////////////////////////////////////////////////////////////
// Headers
////////////////////////////////////////////////////////////
#include "PointPath.hpp"

#ifdef _DEBUG
#include <SFML/Graphics.hpp>
#include <SFML/Window.hpp>

#define WAYPOINT_SIZE 3.0f
#endif

#include <SmegTools/Debug/DebugNew.hpp>

namespace smeg {
    namespace ai {

////////////////////////////////////////////////////////////
/// Default Constructor
////////////////////////////////////////////////////////////
PointPath::PointPath() :
    m_Waypoints(),
    m_CurrentWaypoint( m_Waypoints.end() ),
    m_eLoopType( eNoLoop ),
    m_bIsReverse( false )
{
}

////////////////////////////////////////////////////////////
/// Constructor with a base path
////////////////////////////////////////////////////////////
PointPath::PointPath( const std::vector< sf::Vector2f> _BasePath, eLoopType _eLoop, bool _bIsReverse ) :
    m_Waypoints( _BasePath ),
    m_CurrentWaypoint( m_Waypoints.begin() ),
    m_eLoopType( _eLoop ),
    m_bIsReverse( _bIsReverse )
{
}

////////////////////////////////////////////////////////////
/// Copy Constructor
////////////////////////////////////////////////////////////
PointPath::PointPath( const PointPath& _Copy ) :
    m_Waypoints( _Copy.m_Waypoints ),
    m_CurrentWaypoint( m_Waypoints.begin() ),
    m_eLoopType( _Copy.m_eLoopType ),
    m_bIsReverse( _Copy.m_bIsReverse )
{
}

////////////////////////////////////////////////////////////
/// Destructor
////////////////////////////////////////////////////////////
PointPath::~PointPath()
{
}

////////////////////////////////////////////////////////////
/// Add a waypoint to the end
////////////////////////////////////////////////////////////
void PointPath::Setup( const std::vector< sf::Vector2f> _BasePath, eLoopType _eLoop, bool _bIsReverse ) {
    m_Waypoints         = _BasePath;
    m_eLoopType         = _eLoop;
    m_bIsReverse        = _bIsReverse;

    Start();
}

////////////////////////////////////////////////////////////
/// Operator=
////////////////////////////////////////////////////////////
PointPath& PointPath::operator=( const PointPath& _Copy ) {
    Setup(  _Copy.m_Waypoints,
            _Copy.m_eLoopType,
            _Copy.m_bIsReverse );

    return *this;
}

////////////////////////////////////////////////////////////
/// Add a waypoint to the end
////////////////////////////////////////////////////////////
void PointPath::AddWaypoint( const sf::Vector2f _Waypoint ) {
    m_Waypoints.push_back( _Waypoint );
    m_CurrentWaypoint = m_Waypoints.begin();
}

////////////////////////////////////////////////////////////
/// Start to the current waypoint
///
////////////////////////////////////////////////////////////
void PointPath::Start() {
    if ( m_bIsReverse ) {
        m_CurrentWaypoint   = m_Waypoints.end();
        --m_CurrentWaypoint;
    } else {
        m_CurrentWaypoint   = m_Waypoints.begin();
    }  
}

////////////////////////////////////////////////////////////
/// Loop the path
////////////////////////////////////////////////////////////
void PointPath::Loop() {
    if ( m_bIsReverse ) {
        if ( m_eLoopType == eBackAndForth ) {
            ReverseOff();
            m_CurrentWaypoint   = m_Waypoints.begin();
            ++m_CurrentWaypoint;
        } else {
            m_CurrentWaypoint   = m_Waypoints.end();
            --m_CurrentWaypoint;
        }
    } else {
        if ( m_eLoopType == eBackAndForth ) {
            ReverseOn();
            m_CurrentWaypoint   = m_Waypoints.end();
            // Back 2 times in order to avoid going to the end waypoint again
            --m_CurrentWaypoint;
            --m_CurrentWaypoint;
        } else {
            m_CurrentWaypoint   = m_Waypoints.begin();
        }
    }    
}

////////////////////////////////////////////////////////////
/// Go to the next waypoint
////////////////////////////////////////////////////////////
void PointPath::SetNextWaypoint() {
    if ( IsFinished() ) {
        if ( m_eLoopType != eNoLoop ) {
            Loop();
        }
    } else {
        if ( m_bIsReverse ) {
            --m_CurrentWaypoint;
        } else {
            ++m_CurrentWaypoint;
        }
    }
}

#ifdef _DEBUG
void PointPath::DebugRender( sf::RenderWindow* _pRenderWindow ) const {
    sf::CircleShape waypoint(WAYPOINT_SIZE);
    waypoint.setFillColor( sf::Color::Blue );
    waypoint.setOrigin( WAYPOINT_SIZE, WAYPOINT_SIZE );

    // Draw first point
    waypoint.setPosition( m_Waypoints[0] );
    _pRenderWindow->draw( waypoint );

    // Draw other points and the line between them
    for ( unsigned i = 1; i < m_Waypoints.size(); ++i ) {
        // Draw point
        waypoint.setPosition( m_Waypoints[i] );

        _pRenderWindow->draw( waypoint );

        // Draw line
        sf::Vertex waypointLine[] =
            {
                sf::Vertex( m_Waypoints[i - 1], sf::Color::Blue ),
                sf::Vertex( m_Waypoints[i], sf::Color::Blue )
            };
            
        _pRenderWindow->draw( waypointLine, 2, sf::Lines );
    }

    // Draw current point
    waypoint.setFillColor( sf::Color::Red );
    waypoint.setPosition( *m_CurrentWaypoint );
    _pRenderWindow->draw( waypoint );
}
#endif // _DEBUG

    } // namespace ai
} // namespace smeg